%画人工势场示意图
clc
close all
clear

figure(1)
impRgb=imread('Fruit Forest3.png');%将真彩色图像RGB转换为灰度强度图像
imshow(impRgb);
axis on;
set(gca,'YDir','normal'); %改变y轴递增方向
xlabel x;
ylabel y;
xL=size(impRgb,2);%地图x轴长
yL=size(impRgb,1);%地图y轴长
hold on
k=1;
nums=[];
for i=2:xL
    if (impRgb(280,i)>impRgb(280,i-1) && impRgb(280,i-1)==0)%black->white
%        plot(i,280,'.','MarkerFaceColor','r','MarkerSize',20);
        nums(k,1) = i;
        nums(k,2) = impRgb(280,i)>impRgb(280,i-1);
        k=k+1;
    elseif (impRgb(280,i)<impRgb(280,i-1) && impRgb(280,i-1)==255)%white->black
%         plot(i,280,'.','MarkerFaceColor','r','MarkerSize',20);
        nums(k,1) = i;
        nums(k,2) = impRgb(280,i)>impRgb(280,i-1);
        k=k+1;
    end   
end
k=1;
aveX = [];
for i=2:length(nums) 
    if nums(i)>nums(i-1)
        aveX(k) = (nums(i)+nums(i-1))/2.0;
        k=k+1;
    end
end
aveY=[];
for i=2:yL
    if (impRgb(i,50)>impRgb(i-1,50) && impRgb(i-1,50)==0)%black->white
%         plot(50,i,'.','MarkerFaceColor','r','MarkerSize',20);
        aveY(1) = i;
        aveY(3) = i;
        aveY(5) = i;
    end

    if (impRgb(i,150)<impRgb(i-1,150) && impRgb(i-1,150)==255)%white->black
%         plot(150,i,'.','MarkerFaceColor','r','MarkerSize',20);
        aveY(2) = i;
        aveY(4) = i;
    end
end

for k=1:3
    spacing_ = 10;
    Y_row = linspace(8,298,20);
    Y_col = ones(7,1);
    Y = (Y_col*Y_row)';
    X1_row = linspace(63.5+(k-1)*152-39,63.5+(k-1)*152,7);
    X2_col = ones(20,1);
    X1 = (X2_col*X1_row);
    
    for i=1:size(Y,1)  %Y
       for j=1:size(X1,2)  %X
          Z1(i,j) = sqrt((X1(i,size(X1,2)-j+1)-X1(i,1))^2);
       end
    end
%     surf(X1,Y,ones(20,7)*39-Z1)
%     mesh(X1,Y,Z1)
%     shading facted
    shading interp
%     shading flat
    [DX1,DY] = gradient(Z1,spacing_);
    quiver(X1,Y,DX1,DY,0.4,'g','LineWidth',1.8)%绘制梯度
    hold on    
    
    X2_row = linspace(63.5+(k-1)*152,63.5+(k-1)*152+39,7);
    X2_col = ones(20,1);
    X2 = (X2_col*X2_row);
    for i=1:size(Y,1)  %Y
       for j=1:size(X2,2)  %X
          Z2(i,j) = sqrt((X2(i,j)-X2(i,1))^2);
       end
    end
%     surf(X2,Y,ones(20,7)*39-Z2)
%     mesh(X2,Y,Z2)
%     shading facted
    shading interp
%     shading flat
    [DX2,DY] = gradient(Z2,spacing_);
    quiver(X2,Y,DX2,DY,0.4,'b','LineWidth',1.8)%绘制梯度
    hold on
end

for k=1:2
    spacing_ = 10;
    Y_row = linspace(40,329.5,20);
    Y_col = ones(7,1);
    Y = (Y_col*Y_row)';    
    
    X1_row = linspace(139.5+(k-1)*152-39,139.5+(k-1)*152,7);
    X2_col = ones(20,1);
    X1 = (X2_col*X1_row);
    for i=1:size(Y,1)  %Y
       for j=1:size(X1,2)  %X
          Z1(i,j) = sqrt((X1(i,size(X1,2)-j+1)-X1(i,1))^2);
       end
    end
%     surf(X1,Y,ones(20,7)*39-Z1)
%     mesh(X1,Y,Z1)
%     shading facted
    shading interp
%     shading flat
    [DX1,DY] = gradient(Z1,spacing_);
    quiver(X1,Y,DX1,DY,0.4,'g','LineWidth',1.8)%绘制梯度
    hold on 
    
    X2_row = linspace(139.5+(k-1)*152,139.5+(k-1)*152+39,7);
    X2_col = ones(20,1);
    X2 = (X2_col*X2_row);
    for i=1:size(Y,1)  %Y
       for j=1:size(X2,2)  %X
          Z2(i,j) = sqrt((X2(i,j)-X2(i,1))^2);
       end
    end
%     surf(X2,Y,ones(20,7)*39-Z2)
%     mesh(X2,Y,Z2)
%     shading facted
    shading interp
%     shading flat
    [DX2,DY] = gradient(Z2,spacing_);
    quiver(X2,Y,DX2,DY,0.4,'b','LineWidth',1.8)%绘制梯度
    hold on    
end

lgd = legend('artificial potential field left','artificial potential field right');
lgd.FontSize = 12;
lgd.TextColor = 'blue';
lgd.NumColumns = 1;
lgd.Location = 'northeast';
leg.Orientation = 'vertical';
% legend
% zlabel z;
% figure(2)
% spacing = 0.2;
% [X,Y] = meshgrid(-2:spacing:2);
% Z = X.*exp(-X.^2 - Y.^2);
% [DX,DY] = gradient(Z,spacing);
% 
% quiver(X,Y,DX,DY,0.1)%绘制梯度
% hold on
% contour(X,Y,Z)%绘制等高线
% axis equal
% hold off